

#include <Servo.h>


Servo servoElevation; Servo servoAzimuth; 




#define NoOfPos 30
int azPosition[NoOfPos];
int elvPosition[NoOfPos];

void setup() {

  pinMode(1,OUTPUT);
  servoElevation.attach(4); 
  
  servoAzimuth.attach(3); 
  Serial.begin(9600);
  Serial.println("ReadyReady");
  Serial.println("ReadyReady");
  
  setupAzPos(1200, 1580, NoOfPos);
  setupElvPos(1790, 1550, NoOfPos);

}

void loop() {

  static int v = 0;

  if ( Serial.available()) {
    char ch = Serial.read();

    switch(ch) {
      case '0'...'9':
        v = v * 10 + ch - '0';
        break;
      case 's':
        servoElevation.write(v);
        v = 0;
        break;
      case 'w':
        servoAzimuth.write(v);
        v = 0;
        break;
    }
  }
    
  stepAllPosAndBack(1000);  

} 

void setupAzPos(int fromPos, int toPos, int anzPos) {
  for (int arrIdx=0;arrIdx<anzPos;arrIdx++) {
       azPosition[arrIdx]=fromPos+(arrIdx*(toPos-fromPos))/(anzPos-1);
  }
}

void setupElvPos(int fromPos, int toPos, int anzPos) {
  for (int arrIdx=0;arrIdx<anzPos;arrIdx++) {
       elvPosition[arrIdx]=fromPos+(arrIdx*(toPos-fromPos))/(anzPos-1);
  }
}

void stepAllPosAndBack(int delayMs) {
  for (int i=0;i<NoOfPos;i++) {
      setXYPos(elvPosition[i], azPosition[i], delayMs);
  }
  
  for (int i=NoOfPos-1;i>-1;i--) {
      setXYPos(elvPosition[i], azPosition[i], delayMs);
  }
  
}

void stepFromTo(int fromAz, int azInc, int toAz, int fromElv, int elvInc, int toElv, int delayMs) {
  
  /*
  int azPos=fromAz;
  int elvPos=fromElv;
  if (azPos<=toAz) {
    azPos+=azInc;
  }
  if (elvPos<=toElv
  for (int i=0;i<NoOfPos;i++) {
      setXYPos(elvPosition[i], azPosition[i], delayMs);
  }
  
  for (int i=NoOfPos-1;i>-1;i--) {
      setXYPos(elvPosition[i], azPosition[i], delayMs);
  }
  */
  
}

void setXYPos(int elevation, int azimuth, int delayMillis) {
  servoElevation.write(elevation);
  servoAzimuth.write(azimuth);  
  delay(delayMillis); 
}

